#include "paths_control.h"


Paths_Control::Paths_Control()
    : Start_local_planning("0")
{

    Mpc.Init();

    pub_speed_control = nh.advertise<geometry_msgs::Vector3Stamped>("/xyl/car_control", 10);

    sub_Odom = nh.subscribe("/odom", 10, &Paths_Control::Odom, this);
    sub_MoveBaseSimpleGoal = nh.subscribe("/move_base_simple/goal", 10 ,&Paths_Control::MoveBaseSimpleGoal, this);
    sub_trajectory = nh.subscribe("/xyl/local_paths", 10, &Paths_Control::Trajectory, this);
    sub_speed = nh.subscribe("/xyl/local_velocity", 10, &Paths_Control::Speed, this);
    sub_acceleration = nh.subscribe("/xyl/local_acceleration", 10, &Paths_Control::Acceleration, this);
    sub_theta = nh.subscribe("/xyl/local_theta", 10, &Paths_Control::Theta, this);
    sub_kappas = nh.subscribe("/xyl/local_kappas", 10, &Paths_Control::Kappas, this);

    sub_Start_local_ = nh.subscribe("/xyl/planning/Start_Dynamic", 10, &Paths_Control::LocalStart, this);

    public_control_thread_ = std::make_unique<std::thread>(&Paths_Control::public_control_thread, this);
}

Paths_Control::~Paths_Control(){
    if(public_control_thread_ && public_control_thread_->joinable()){
        public_control_thread_->join();
    }
}

void Paths_Control::public_control_thread(){
    ros::Rate loop_rate(10);
    double turn_angle = 0;
    double out_index_ = 0;
    geometry_msgs::Vector3Stamped msg_ros;
    while (nh.ok())
    {
        if(is_move_base_simple_goal == true && Start_local_planning == "1"){
            is_vehical_stop_set =   Vehical_Stop(goal_pose_, odom_);
            // Lqr.lqr_control(odom_, loacl_paths, car_kappas, car_theta, car_speed, turn_angle, out_index_);
            Mpc.mpc_control(odom_, loacl_paths, car_kappas, car_theta, car_speed, turn_angle, out_index_);
            msg_ros.header.stamp = ros::Time::now();
            msg_ros.header.frame_id = "map";
            msg_ros.vector.y = turn_angle;          // 转角
            if(is_vehical_stop_set == false){
                if(out_index_ < car_speed.size()){
                    // msg_ros.vector.x = car_speed[out_index_];
                    msg_ros.vector.x = out_index_;
                    LOG(WARNING) << "speed: " << msg_ros.vector.x;
                    pub_speed_control.publish(msg_ros);
                }else{
                    msg_ros.vector.x = 0.0; // ros仿真直接赋值速度
                    pub_speed_control.publish(msg_ros);
                }
            }else{
                ROS_WARN("Arrive at goal");
                msg_ros.vector.x = 0.0; // ros仿真直接赋值速度
                pub_speed_control.publish(msg_ros);
                is_move_base_simple_goal = false;
                Start_local_planning ="0";
            }
        }else if (is_move_base_simple_goal == true && Start_local_planning == "2"){
            msg_ros.vector.x = 0;
            pub_speed_control.publish(msg_ros);
        }
       
       ros::spinOnce();
       loop_rate.sleep();
    }
}

bool Paths_Control::Vehical_Stop(const geometry_msgs::Pose &goal_point, nav_msgs::Odometry &odom)
{
  // 获取车的里程位置
  double x = odom.pose.pose.position.x;
  double y = odom.pose.pose.position.y;
  // 获取路径点的最后一个位置
  double xt = goal_point.position.x;
  double yt = goal_point.position.y;
  // 判断如果两点坐标接近

  double dis = sqrt((x - xt) * (x - xt) + (y - yt) * (y - yt));
  if (dis < 0.3)
  {
    return true;
  }
  return false;
}

void Paths_Control::Trajectory(const nav_msgs::Path::ConstPtr &trajectory_msg){
    loacl_paths.poses.clear();
    loacl_paths.header.frame_id = "map";
    loacl_paths.header.stamp = ros::Time::now();
    ros::Time current_time = ros::Time::now();
    for(size_t i = 0; i < trajectory_msg->poses.size(); i++){
        geometry_msgs::PoseStamped pose_stamp;
        pose_stamp.header.frame_id = "map";
        pose_stamp.header.stamp = current_time;
        pose_stamp.pose.position.x = trajectory_msg->poses[i].pose.position.x;
        pose_stamp.pose.position.y = trajectory_msg->poses[i].pose.position.y;
        pose_stamp.pose.position.z = 0;
        loacl_paths.poses.push_back(pose_stamp);
    }

}

void Paths_Control::Speed(const geometry_msgs::PoseArray::ConstPtr &speed_msg){
    car_speed.clear();
    for(size_t i = 0; i < speed_msg->poses.size(); i++){
        double x = speed_msg->poses[i].position.x;
        car_speed.push_back(x);
    }
}

void Paths_Control::Acceleration(const geometry_msgs::PoseArray::ConstPtr &acceleration_msg){
    car_acceleration.clear();
    for(size_t i = 0; i < acceleration_msg->poses.size(); i++){
        double x = acceleration_msg->poses[i].position.x;
        car_acceleration.push_back(x);
    }
}

void Paths_Control::Theta(const geometry_msgs::PoseArray::ConstPtr &theta_msg){
    car_theta.clear();
    for(size_t i = 0; i < theta_msg->poses.size(); i++){
        double x = theta_msg->poses[i].position.x;
        car_theta.push_back(x);
    }
}

void Paths_Control::Kappas(const geometry_msgs::PoseArray::ConstPtr &kappas_msg){
    car_kappas.clear();
    for(size_t i = 0; i < kappas_msg->poses.size(); i++){
        double x = kappas_msg->poses[i].position.x;
        car_kappas.push_back(x);
    }
}

void Paths_Control::Odom(const nav_msgs::Odometry &msg)
{
    odom_ = msg;
}

void Paths_Control::MoveBaseSimpleGoal(const geometry_msgs::PoseStamped::ConstPtr &move_base_goal_msg){
    if(Start_local_planning == "0"){
        is_move_base_simple_goal = true;
        goal_pose_.position.x = move_base_goal_msg->pose.position.x;
        goal_pose_.position.y = move_base_goal_msg->pose.position.y;
        goal_pose_.orientation = move_base_goal_msg->pose.orientation;
    }
}

void Paths_Control::LocalStart(const std_msgs::String::ConstPtr &go)
{
    Start_local_planning = go->data.c_str();
}
